#include "winkel.h"

int SuchenWinkel(double sollpunkt[2], double l1, double l2, double alpha1[2],double alpha2[2])
{
	int anz_loes, j;
	static double winkel1_alt=0, hilfs_alpha1, hilfs_alpha2;
	double  k1,a, b, c;
	double  vx, vy, x[2], y[2], l;
	double pi=3.14159;




	k1=sollpunkt[1]*sollpunkt[1]+sollpunkt[0]*sollpunkt[0]+l1*l1-l2*l2;
	a=sollpunkt[1]*sollpunkt[1]+sollpunkt[0]*sollpunkt[0];
	b=-k1*sollpunkt[0];
	c=0.25*k1*k1-l1*l1*sollpunkt[1]*sollpunkt[1];


	if((b*b-4*a*c)/(2*a)>=0)
	{
		x[0]=(-b+sqrt(b*b-4*a*c))/(2*a);//					X-Komponente 1
		x[1]=(-b-sqrt(b*b-4*a*c))/(2*a);//					X-Komponente 2
		y[0]=(-sollpunkt[0] * x[0]+k1/2)/sollpunkt[1];//	Y-Komponente 1
		y[1]=(-sollpunkt[0] * x[1]+k1/2)/sollpunkt[1];//	Y-Komponente 2
		anz_loes=2;//										2 Loesungen

		if (sollpunkt[1]==0)
			{
				y[0]=sqrt(l1*l1-x[0]*x[0]);
				y[1]=-sqrt(l1*l1-x[0]*x[0]);
			}

		if ((b*b-4*a*c)/(2*a)==0)
			{
			anz_loes=1;
			}
	}
	else
	{
		anz_loes=0;
		return(anz_loes);
	}


	for (j=0; j<=1; j++)
		{

			vx=x[j];
			vy=y[j];
			l=l1;

			if (vx>=0)
				{
				alpha1[j]=acos(vy/l)*360/(2*pi);
				if (alpha1[j]<0) alpha1[j]=alpha1[j]+360;
				}

			else //vx<0
				{
				alpha1[j]=-acos(vy/l)*360/(2*pi);
				if (alpha1[j]<0) alpha1[j]=alpha1[j]+360;
				}

			vx=sollpunkt[0]-x[j];
			vy=sollpunkt[1]-y[j];
			l=l2;


			if (vx>=0)
				{
				alpha2[j]=acos(vy/l)*360/(2*pi);
				if (alpha2[j]<0) alpha2[j]=alpha2[j]+360;
				}

			else  //vx<0
				{
				alpha2[j]=-acos(vy/l)*360/(2*pi);
				if (alpha2[j]<0) alpha2[j]=alpha2[j]+360;
				}
		} // for...


	if ((winkel1_alt-alpha1[0])*(winkel1_alt-alpha1[0]) >(winkel1_alt-alpha1[1])*(winkel1_alt-alpha1[1]) )
		{
			hilfs_alpha1=alpha1[0];// falls der Roboterarm hier einen Orientierungswechsel machen möchte, wird das hier abgefangen
			hilfs_alpha2=alpha2[0];
			alpha1[0]=alpha1[1];
			alpha2[0]=alpha2[1];
			alpha1[1]=hilfs_alpha1;
			alpha2[1]=hilfs_alpha2;
			printf("\n2. Winkelpaar");
		} // if winkel1_alt
		else
			{
				printf("\n1. Winkelpaar");
			}

	winkel1_alt=alpha1[0];
	return (anz_loes);// Anzahl der Lösungen (keine, eine oder 2) wird als int-Wert zurückgegeben
}
